motion_buffer.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
#include "core/common/wolf.h"
#include "core/common/time_stamp.h"
 
#include <list>
#include <algorithm>
 
namespace wolf
{
struct Motion
{
  public:
    SizeEigen       data_size_, delta_size_, cov_size_, calib_size_;
    TimeStamp       ts_;                     
    Eigen::VectorXd data_;                   
    Eigen::MatrixXd data_cov_;               
    Eigen::VectorXd delta_;                  
    Eigen::MatrixXd delta_cov_;              
    Eigen::VectorXd delta_integr_;           
    Eigen::MatrixXd delta_integr_cov_;       
    Eigen::MatrixXd jacobian_delta_;         
    Eigen::MatrixXd jacobian_delta_integr_;  
    Eigen::MatrixXd jacobian_calib_;  
  public:
    Motion() = delete;  // completely delete unpredictable stuff like this
    Motion(const TimeStamp& _ts,
           SizeEigen        _data_size,
           SizeEigen        _delta_size,
           SizeEigen        _cov_size,
           SizeEigen        _calib_size);
    Motion(const TimeStamp&       _ts,
           const Eigen::VectorXd& _data,
           const Eigen::MatrixXd& _data_cov,
           const Eigen::VectorXd& _delta,
           const Eigen::MatrixXd& _delta_cov,
           const Eigen::VectorXd& _delta_int,
           const Eigen::MatrixXd& _delta_integr_cov,
           const Eigen::MatrixXd& _jac_delta,
           const Eigen::MatrixXd& _jac_delta_int,
           const Eigen::MatrixXd& _jacobian_calib);
    ~Motion();
 
  private:
    void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
 
};  
 
class MotionBuffer : public std::list<Motion>
{
  public:
    MotionBuffer();
    const Motion& getMotion(const TimeStamp& _ts) const;
    void          getMotion(const TimeStamp& _ts, Motion& _motion) const;
    void          split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
    void          print(bool show_data      = 0,
                        bool show_delta     = 0,
                        bool show_delta_int = 0,
                        bool show_jacs      = 0,
                        bool show_covs      = 0);
};
 
}  // namespace wolf